package colbotcanon;

import gnu.io.CommPortIdentifier;
import gnu.io.PortInUseException;
import gnu.io.SerialPort;
import gnu.io.UnsupportedCommOperationException;
import info.monitorenter.gui.chart.Chart2D;
import info.monitorenter.gui.chart.ITrace2D;
import info.monitorenter.gui.chart.rangepolicies.RangePolicyFixedViewport;
import info.monitorenter.gui.chart.traces.Trace2DSimple;
import info.monitorenter.util.Range;

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Container;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;
import java.io.IOException;
import java.io.OutputStream;
import java.util.Arrays;
import java.util.Enumeration;
import java.util.logging.Level;
import java.util.logging.Logger;

import javax.swing.BorderFactory;
import javax.swing.JButton;
import javax.swing.JCheckBox;
import javax.swing.JFrame;
import javax.swing.JSlider;
import javax.swing.event.ChangeEvent;
import javax.swing.event.ChangeListener;

import ros.NodeHandle;
import ros.Ros;
import ros.Subscriber;
import ros.pkg.sensor_msgs.msg.LaserScan;

/**
 * Control the Colbot Canon
 * 
 * @author Frederic Pesquet (fpesquet at gmail dot com)
 */
public class ColbotCanonNode extends JFrame {
	private static final long serialVersionUID = 1L;

	protected static final Logger LOG = Logger.getLogger(ColbotCanonNode.class
			.getName());

	/**
	 * Name of the serial port used to talk to the Arduino controller which
	 * handles the robot head pan servo
	 */
	static private final String _arduinoSerialPortName = "/dev/ttyUSB0";

	private static final int _laserScanNbSamples = 514;

	private static final double _laserScanMaxRange = 6; // in meters

	/** serial port to arduino board */
	private SerialPort _arduinoSerialPort;

	/** serial stream */
	private OutputStream _arduinoSerialPortOutput;

	// private InputStream _arduinoSerialPortInput;

	private final NodeHandle _rosNodeHandle;

	private final Ros _ros;

	private ITrace2D _laserScanTrace;

	private ITrace2D _laserScanDiffTrace;

	private float[] _referenceLaserRanges;

	public ColbotCanonNode() throws Exception {
		super("Colbot Canon");

		_ros = Ros.getInstance();
		_ros.init("ColbotCanonNode");
		_rosNodeHandle = _ros.createNodeHandle();
		Subscriber.QueueingCallback<LaserScan> callback = new Subscriber.QueueingCallback<LaserScan>() {
			@Override
			public void call(LaserScan msg) {
				evtLaserScanReceived(msg);
			}
		};
		_rosNodeHandle.subscribe("/scan", new LaserScan(), callback, 10);

		new Thread("ROS_listen") {
			@Override
			public void run() {
				_rosNodeHandle.spin();
			}

		}.start();

		initGUI();

		connectToArduinoCtrl();

		addWindowListener(new WindowAdapter() {
			@Override
			public void windowClosed(WindowEvent e) {
				shutdown();
			}
		});
	}

	private void shutdown() {
		disconnectFromArduinoCtrl();
		_rosNodeHandle.shutdown();
	}

	private void evtLaserScanReceived(LaserScan msg) {
		_laserScanTrace.removeAllPoints();
		for (int n = 0; n < _laserScanNbSamples; n++) {
			double angleScanInDegree = ((double) n / _laserScanNbSamples) * 180.0 - 90.0;
			_laserScanTrace.addPoint(angleScanInDegree, msg.ranges[n]);
		}

		if (_referenceLaserRanges != null) {
			_laserScanDiffTrace.removeAllPoints();
			double clipDiffValue = _laserScanMaxRange / 2 - 0.1;
			for (int n = 0; n < _laserScanNbSamples; n++) {
				double angleScanInDegree = ((double) n / _laserScanNbSamples) * 180.0 - 90.0;
				double diffV = 0.0;
				if ((msg.ranges[n] != 0) && (_referenceLaserRanges[n] != 0)) {
					diffV = (_referenceLaserRanges[n] - msg.ranges[n]);
					if (diffV > clipDiffValue)
						diffV = clipDiffValue;
					if (diffV < -clipDiffValue)
						diffV = -clipDiffValue;
				}
				diffV = diffV + _laserScanMaxRange / 2; // center in middle
				_laserScanDiffTrace.addPoint(angleScanInDegree, diffV);
			}
		}
		_referenceLaserRanges = Arrays.copyOf(msg.ranges, msg.ranges.length);
	}

	@SuppressWarnings("unchecked")
	private void connectToArduinoCtrl() throws IOException, PortInUseException,
			UnsupportedCommOperationException {
		Enumeration<CommPortIdentifier> portList = CommPortIdentifier
				.getPortIdentifiers();
		while (portList.hasMoreElements()) {
			CommPortIdentifier portId = portList.nextElement();
			if (portId.getName().equals(_arduinoSerialPortName)) {
				LOG.info("open_port:" + portId.getName());
				_arduinoSerialPort = (SerialPort) portId.open("Arduino Serial",
						2000);
				// _serialPort.setFlowControlMode(SerialPort.FLOWCONTROL_NONE);
				// _arduinoSerialPortInput =
				// _arduinoSerialPort.getInputStream();
				_arduinoSerialPortOutput = _arduinoSerialPort.getOutputStream();
				_arduinoSerialPort.setSerialPortParams(9600, 8,
						SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
			}
		}
		if (_arduinoSerialPort == null) {
			throw new IOException("Can't connect to Arduino");
		}
	}

	private void disconnectFromArduinoCtrl() {
		try {
			if (_arduinoSerialPort != null) {
				LOG.log(Level.INFO, "closing serial port");
				_arduinoSerialPort.close();
			}
		} catch (Exception e) {
			LOG.log(Level.WARNING, "can't close serial port", e);
		}
		_arduinoSerialPort = null;
	}

	/**
	 * Set desired position for given servo
	 * 
	 * @param servo
	 *            one of 'F' (fire), 'P' (pan), 'T' (tilt)
	 * @param anglerad
	 *            desired angle, in radians. 0 = middle.
	 */
	private void setServoTo(char servo, double anglerad) {
		try {
			int servoAngle = (int) Math.toDegrees(anglerad) + 90;
			if (servoAngle < 0)
				servoAngle = 0;
			if (servoAngle > 180)
				servoAngle = 180;
			LOG.log(Level.INFO, "set angle for " + servo + " to " + servoAngle);
			_arduinoSerialPortOutput.write('S');
			_arduinoSerialPortOutput.write(servo);
			_arduinoSerialPortOutput.write(servoAngle);
			_arduinoSerialPortOutput.flush();
		} catch (Exception e) {
			LOG.log(Level.WARNING, "Error occurred writing to arduino", e);
		}
	}

	/**
	 * Set desired speed for motor (-127..127)
	 */
	private void setMotorTo(int speed) {
		try {
			LOG.log(Level.INFO, "set motor speed to " + speed);
			_arduinoSerialPortOutput.write('S');
			_arduinoSerialPortOutput.write('M');
			_arduinoSerialPortOutput.write(speed);
			_arduinoSerialPortOutput.flush();
		} catch (Exception e) {
			LOG.log(Level.WARNING, "Error occurred writing to arduino", e);
		}
	}

	/**
	 * A simple panel with some buttons
	 */
	private void initGUI() {
		Container content = getContentPane();

		Chart2D chart = new Chart2D();
		chart.getAxisX().setRangePolicy(
				new RangePolicyFixedViewport(new Range(-90, 90)));
		chart.getAxisY().setRangePolicy(
				new RangePolicyFixedViewport(new Range(0, _laserScanMaxRange)));
		_laserScanTrace = new Trace2DSimple("Laser Scans");
		_laserScanTrace.setColor(Color.RED);
		chart.addTrace(_laserScanTrace);
		_laserScanDiffTrace = new Trace2DSimple("Laser Scans Diffs");
		_laserScanDiffTrace.setColor(Color.BLUE);
		chart.addTrace(_laserScanDiffTrace);

		content.add(chart, BorderLayout.CENTER);

		JButton fireS = new JButton("Fire");
		fireS.addActionListener(new ActionListener() {
			@Override
			public void actionPerformed(ActionEvent arg0) {
				setServoTo('F', Math.toRadians(9));
				try {
					Thread.sleep(500);
				} catch (InterruptedException e) {
					e.printStackTrace();
				}
				setServoTo('F', Math.toRadians(39));
			}
		});
		content.add(fireS, BorderLayout.EAST);

		JSlider panS = new JSlider(-90, 90, 0);
		panS.addChangeListener(new ChangeListener() {
			public void stateChanged(ChangeEvent evt) {
				JSlider slider = (JSlider) evt.getSource();
				if (!slider.getValueIsAdjusting()) {
					int value = slider.getValue();
					setServoTo('P', Math.toRadians(value));
				}
			}
		});
		panS.setBorder(BorderFactory.createTitledBorder("Canon Pan"));
		panS.setMajorTickSpacing(10);
		panS.setMinorTickSpacing(1);
		panS.setPaintTicks(true);
		panS.setPaintLabels(true);
		content.add(panS, BorderLayout.NORTH);

		JSlider tiltS = new JSlider(-90, 90, 0);
		tiltS.addChangeListener(new ChangeListener() {
			public void stateChanged(ChangeEvent evt) {
				JSlider slider = (JSlider) evt.getSource();
				if (!slider.getValueIsAdjusting()) {
					int value = slider.getValue();
					setServoTo('T', Math.toRadians(value));
				}
			}
		});
		tiltS.setBorder(BorderFactory.createTitledBorder("Canon Tilt"));
		tiltS.setMajorTickSpacing(10);
		tiltS.setMinorTickSpacing(1);
		tiltS.setPaintTicks(true);
		tiltS.setPaintLabels(true);
		content.add(tiltS, BorderLayout.SOUTH);

		final JCheckBox motorS = new JCheckBox("Canon Motor");
		motorS.addActionListener(new ActionListener() {
			@Override
			public void actionPerformed(ActionEvent arg0) {
				setMotorTo((motorS.isSelected() ? 200 : 0));
			}
		});
		content.add(motorS, BorderLayout.WEST);

		pack();
	}

	/**
	 * Entry point: create the frame, and wait until closed.
	 */
	public static void main(String[] args) throws Exception {
		ColbotCanonNode snd = new ColbotCanonNode();
		snd.setSize(800, 400);
		snd.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		snd.setLocationRelativeTo(null);
		snd.setVisible(true);
	}
}
